package projekt.midterRobot;

import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;

import lejos.nxt.*;
import lejos.nxt.comm.BTConnection;
import lejos.nxt.comm.Bluetooth;
import lejos.nxt.comm.NXTConnection;

public class MidterRobotMain {

	public static final int MOTOR_A_C_STOP = 0;
	public static final int MOTOR_A_C_FORWARD = 1;
	public static final int MOTOR_A_C_BACKWARD = 2;
	public static final int MOTOR_A_FORWARD = 3;
	public static final int MOTOR_A_BACKWARD = 4;
	public static final int MOTOR_C_FORWARD = 5;
	public static final int MOTOR_C_BACKWARD = 6;    
	public static final int MOTOR_C_STOP = 7;    
	public static final int MOTOR_A_STOP = 8;    
	public static final int POWER_UP=9;
	public static final int POWER_DOWN=10;
	public static final int DISCONNECT = 99; 

	public static void main(String [] args) throws IOException, InterruptedException
	{  	    
		int power = +50;

		Button.ESCAPE.addButtonListener(new ButtonListener() {
			@Override public void buttonReleased(Button b) { System.exit(0); }
			@Override public void buttonPressed(Button b) {}
		});

		String connected = "Connected";
		String waiting = "Waiting...";
		String closing = "Closing...";  

		while (true)
		{
			LCD.drawString(waiting,0,0);
			LCD.refresh();

			BTConnection btc = Bluetooth.waitForConnection(0, NXTConnection.RAW);

			LCD.clear();
			LCD.drawString(connected,0,0);
			LCD.refresh();    

			DataInputStream dis = btc.openDataInputStream();
			DataOutputStream dos = btc.openDataOutputStream();

			while(true)
			{
				int command = (int) dis.readByte();

				switch (command) {
				case MOTOR_A_FORWARD: 
					Motor.B.forward();
					LCD.clear();
					LCD.drawString("Forward, power: " + power, 0, 0);
					break;
				case MOTOR_A_BACKWARD: 
					Motor.B.backward();
					LCD.clear();
					LCD.drawString("Bacward, power: " + power, 0, 0);
					break;
				case MOTOR_A_STOP: 
					Motor.B.stop();
					LCD.clear();
					LCD.drawString("Stop, power: " + power, 0, 0);
					break;
				case MOTOR_C_FORWARD: 
					Motor.C.forward();
					LCD.clear();
					LCD.drawString("Forward, power: " + power, 0, 0);
					break;
				case MOTOR_C_BACKWARD: 
					Motor.C.backward();
					LCD.clear();
					LCD.drawString("Bacward, power: " + power, 0, 0);
					break;
				case MOTOR_C_STOP: 
					Motor.C.stop();
					LCD.clear();
					LCD.drawString("Stop, power: " + power, 0, 0);
					break;
				case MOTOR_A_C_FORWARD:
					Motor.B.forward();
					Motor.C.forward();
					LCD.clear();
					LCD.drawString("Forward, power: " + power, 0, 0);
					break;
				case MOTOR_A_C_BACKWARD:
					Motor.B.backward();
					Motor.C.backward();
					LCD.clear();
					LCD.drawString("Bacward, power: " + power, 0, 0);
					break;
				case MOTOR_A_C_STOP:
					Motor.B.stop();
					Motor.C.stop();
					LCD.clear();
					LCD.drawString("Stop, power: " + power, 0, 0);
					break;
				case POWER_DOWN:
					Motor.B.setPower(power -= 10);
					Motor.C.setPower(power);
					LCD.clear();
					LCD.drawString("Down, power: " + power, 0, 0);
					break;
				case POWER_UP:
					Motor.B.setPower(power += 10);
					Motor.C.setPower(power);
					LCD.clear();
					LCD.drawString("Up, power: " + power, 0, 0);
					break;
				case DISCONNECT: 
					LCD.clear();
					LCD.drawString("Disconnect, power: " + power, 0, 0);
					break;
				default: 
//					Motor.A.stop();    
//					Motor.C.stop();  
					LCD.clear();
					LCD.drawString("Error, power: " + power, 0, 0);                     
				}
				if (command == DISCONNECT) break;
				LCD.drawString("Command: " + command, 0, 1);
			}

			dis.close();
			dos.close();
			Thread.sleep(100); // wait for data to drain
			LCD.clear();
			LCD.drawString(closing,0,0);
			LCD.refresh();
			btc.close();
			LCD.clear();
		}

	}

}
